#include "relay_ctrl.h"
#include "led.h"
#include "flash.h"
#include "upgrade.h"



#define USE_DEBUG_PRINT

#ifndef USE_DEBUG_PRINT  
#define debug_print(...)
#else
#define debug_print(fmt,...) printf("[%d]"fmt,My_GetTick(),##__VA_ARGS__)
#endif

// uint32_t break_up_register __attribute__((section(".ARM.__at_0x801F800")));
const static uint32_t break_up_register = 0x801F004;

//#define MAX_
gpio_pair_t relay_ctrl_gpios[MAX_RELAY_NUM] = {
    {GPIOB,GPIO_PIN_7},
    {GPIOB,GPIO_PIN_6},
    {GPIOB,GPIO_PIN_5},
    {GPIOB,GPIO_PIN_4},
    {GPIOB,GPIO_PIN_3}
};




bool low_power_flag = false;
//拆机事件标志,0xFA代表拆机事件
uint8_t abnormal_flag = 0;
//开关机标志
bool open_close_flag = false;
//急停标志
bool estop_flag = false;
//运动部件安全标志
bool is_motion_part_safe = false;

static bool isDisassembleFlagUpdated = false;

SYS_STATE_t sys_state = SYS_NORMAL_STATE;

void relay_ctrl_gpio_init(void)
{
    rcu_periph_clock_enable(RCU_GPIOB);

    for(u8 i=0;i<MAX_RELAY_NUM;i++)
    {
        gpio_mode_set(relay_ctrl_gpios[i].gpio_periph, GPIO_MODE_OUTPUT, GPIO_PUPD_PULLDOWN, relay_ctrl_gpios[i].pin);
        gpio_output_options_set(relay_ctrl_gpios[i].gpio_periph, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, relay_ctrl_gpios[i].pin);

        gpio_bit_reset(relay_ctrl_gpios[i].gpio_periph, relay_ctrl_gpios[i].pin);
    }
}

uint8_t relay_ctrl(RELAY_GPIO_NAME WHICH_RELAY,RELAY_CTRL_STATE ON_OFF)
{
    if(ON_OFF == ON)
    {
        gpio_bit_set(relay_ctrl_gpios[WHICH_RELAY].gpio_periph,relay_ctrl_gpios[WHICH_RELAY].pin);
    }
    else
    {
        gpio_bit_reset(relay_ctrl_gpios[WHICH_RELAY].gpio_periph, relay_ctrl_gpios[WHICH_RELAY].pin);
    }

    return 0;
}

void all_relay_on(void)
{
    for(u8 i=0;i<MAX_RELAY_NUM;i++)
    {
        relay_ctrl(i,ON);
    }
}

void all_relay_off(void)
{
    for(u8 i=0;i<MAX_RELAY_NUM;i++)
    {
        relay_ctrl(i,OFF);
    }
}

/*
    uC_JCQ,
    uC_ALL,
    uC_JRP,
    FRD_CTL
    以上控制关闭
*/
static void estop_relay_off(void)
{
    relay_ctrl(uC_JCQ,OFF);
    relay_ctrl(uC_ALL,OFF);
    relay_ctrl(uC_JRP,OFF);
    relay_ctrl(FRD_CTL,OFF);
}
 
static void StopSysTick(void) {
    SysTick->CTRL &= ~SysTick_CTRL_ENABLE_Msk;  // 清除ENABLE位以停止SysTick
}

void stby_relay_off(void)
{
    relay_ctrl(uC_JCQ,OFF);
    relay_ctrl(uC_JRP,OFF);
}

void stby_relay_on(void)
{
    relay_ctrl(uC_JCQ,ON);
    relay_ctrl(uC_JRP,ON);
}


static void disable_all_periph(void)
{
    LED_OFF();
    set_rs485_direction(RX);
    rcu_periph_clock_disable(RCU_USART0);
    rcu_periph_clock_disable(RCU_USART1);
    rcu_periph_clock_disable(RCU_GPIOB);
    rcu_periph_clock_disable(RCU_GPIOC);
    rcu_periph_clock_disable(RCU_BKP);
    StopSysTick();
}

static void enter_low_power(void)
{
    pmu_wakeup_pin_enable(PMU_WAKEUP_PIN0);
    rcu_periph_clock_enable(RCU_PMU);
    pmu_to_standbymode();

    // pmu_to_deepsleepmode(PMU_LDNPDSP_LOWDRIVE, WFI_CMD, PMU_DEEPSLEEP2);
}

void record_broken_event(uint8_t value)
{
    uint32_t tmp;

    uint32_t read_value = *(uint32_t*)UPGRADE_FLAG_ADDR;

    fmc_unlock();
    fmc_page_erase(UPGRADE_FLAG_ADDR);
    fmc_word_program(UPGRADE_FLAG_ADDR,read_value);
    if(0xFA == value)
    {
        tmp = 0xAAAAAAAA;
        fmc_word_program(UPGRADE_FLAG_ADDR+4,tmp);
    }

    fmc_lock();

    #if 0
    /* enable PMU and BKP clock */
    rcu_periph_clock_enable(RCU_PMU);
    rcu_periph_clock_enable(RCU_BKP);
    /* enable the access of the RTC registers */
    pmu_backup_write_enable();

    RTC_BKP0 = (uint32_t)value;
    #endif
}

uint8_t read_broken_event(void)
{
    uint32_t tmp = 0;

    flash_read(break_up_register,(uint32_t*)&tmp,1);
    if(0xAAAAAAAA == tmp)
    {
        return 0xFA;
    }
    else
    {
        return 0;
    }

    #if 0
    /* enable PMU and BKP clock */
    rcu_periph_clock_enable(RCU_PMU);
    rcu_periph_clock_enable(RCU_BKP);

    return (uint8_t)RTC_BKP0;
    #endif
}

void init_broken_event(void)
{
    abnormal_flag = read_broken_event();
}

uint16_t relay_ctrl_poll_time_count = 0;
void relay_ctrl_poll(void)
{
    // #define FOR_TEST_ONLY
    #define ENTER_LOW_POWER_MODE_DELAY  10000
    static u8 state_machine = 0;

    switch(state_machine)
    {
        case 0:
        relay_ctrl_poll_time_count = 0;
        state_machine = 1;
        break;

        case 1:
        if(relay_ctrl_poll_time_count >= 1000)
        {
            relay_ctrl_poll_time_count = 0;
            state_machine = 2;
        }
        break;

        case 2:
        #if defined FOR_TEST_ONLY
        //用电源键唤醒待机测试
        if(false == get_state_value(POW_SW_DET))
        #else
        //未检测到24V
        if(true == get_state_value(POW_24V_DET))
        #endif
        {
            // debug_print("24V supply is not ready\r\n");
			relay_ctrl_poll_time_count = 0;
            state_machine = 3;
        }
        else
        {
            // debug_print("24V supply is ready to use\r\n");
            //检测到24V
            low_power_flag = false;
            state_machine = 4;
        }
        break;

        case 3:
        //进入低功耗模式前延时，可以不延时，添加了延时只是为了观察延时效果
        if(relay_ctrl_poll_time_count >= ENTER_LOW_POWER_MODE_DELAY)
        {
            relay_ctrl_poll_time_count = 0;

            //再次确认是否24V信号未检测到，确实没检测到24V时
            if(true == get_state_value(POW_24V_DET))
            {
                if(false == low_power_flag)
                {
                    low_power_flag = true;
                }

                state_machine = 100;
            }
            else
            {
                state_machine = 2;
            }
        }
        else
        {
            //如果在延时时间内检测到24V信号，则返回处理24V供电处理逻辑
            if(false == get_state_value(POW_24V_DET))
            {
                state_machine = 2;
            }
        }
        break;

        case 4:
        if(true == get_state_value(ESTOP_DET))
        {
            //急停未按下
            estop_flag = false;
            state_machine = 5;
        }
        else
        {
            //急停按下
            estop_flag = true;
            state_machine = 100;
        }
        break;

        case 5:
        //WKUP0_DET引脚高电平
        if(true == get_state_value(WKUP0_DET))
        {
            // debug_print("[%d]:",My_GetTick());
            debug_print("WKUP0_DET pin is high\r\n");   
            state_machine = 6;
        }
        else
        {
            debug_print("WKUP0_DET pin is low\r\n");
						open_close_flag = false;
            state_machine = 100;
        }
        break;

        case 6:
        //测试时暂时置true
        // if(true)
        //判断造成WKUP0_DET引脚高电平的是因为拆机事件还是电源开关
        //待电源板V0.2版本引出FC1和FC2至单片机后使用此条件判断
        if((false == get_state_value(FC1_DET)) && (false == get_state_value(FC2_DET)))
        {
            if(false == low_power_flag)
            {
                state_machine = 7;
            }
            else
            {
                nvic_system_reset();
                low_power_flag = false;
                state_machine = 0;
            }
        }
        else
        {
            relay_ctrl_poll_time_count = 0;
            state_machine = 110;

            // //发现拆机事件
            // record_broken_event(0xFA);
            
            // abnormal_flag = read_broken_event();
            // state_machine = 100;
        }
        break;

        case 7:
        if(true == get_state_value(POW_SW_DET))
        {
            debug_print("system start to work signal detect\r\n");
            open_close_flag = true;
            if(sys_state == SYS_NORMAL_STATE)
            {
                all_relay_on();
            }
            
            state_machine = 2;
        }
        else
        {
            sys_state = SYS_NORMAL_STATE;
            debug_print("system power down detect\r\n");
            open_close_flag = false;
            state_machine = 100;
        }
        
        break;

        case 100:
        debug_print("shut down all relays\r\n");
        //此情况只有24V接通才会出现;急停未按下时
        if(true == estop_flag)
        {
            if(false == get_state_value(POW_SW_DET))
            {
                open_close_flag = false;
                all_relay_off();
            }
            else
            {
                estop_relay_off();
            }
        }
        else
        {
            if(false == open_close_flag)
            {
                all_relay_off();
            }
        }
        if(true == low_power_flag)
        {
            //
            debug_print("ready to enter standby mode\r\n");
            disable_all_periph();
            // LED_ON();
            enter_low_power();
            state_machine = 200;
        }
        else
        {
            state_machine = 2;
        }
        break;

        case 110:
        //5秒后再次检测拆机信号
        if(relay_ctrl_poll_time_count >= 5000)
        {
            //如果确实发现了拆机信号，则记录拆机事件
            if((true == get_state_value(FC1_DET)) || (true == get_state_value(FC2_DET)))
            {
                if(true == get_state_value(FC1_DET))
                {
                    printf("FC1 DET\n");
                }
                else
                {
                    printf("FC2 DET\n");
                }
                //发现拆机事件
                record_broken_event(0xFA);
                
                abnormal_flag = read_broken_event();
                state_machine = 100;
            }
            else
            {
                state_machine = 7;
            }
        }
        break;

        default:
        break;
    }
}

/**
 * @brief Check if the device is in disassembled state
 *
 * Checks the status of FC1 and FC2 detection signals to determine
 * if the device has been disassembled
 *
 * @return true if device is disassembled, false otherwise
 */
static inline bool CheckDisassembleState(void)
{
    if ((true == get_state_value(FC1_DET)) || (true == get_state_value(FC2_DET)))
    {
        if(true == get_state_value(FC1_DET))
        {
            // printf("FC1 DET\n");
        }

        if(true == get_state_value(FC2_DET))
        {
            // printf("FC2 DET\n");
        }

        return true;
    }

    return false;
}

/**
 * @brief Check the 24V power supply state
 * @return true if 24V power supply is detected, false otherwise
 */
static inline bool Check24VState(void)
{
    return false == get_state_value(POW_24V_DET);
}

/**
 * @brief Check the emergency stop state
 * @return true if E-stop is active, false otherwise
 */
static inline bool CheckEstopState(void)
{
    return false == get_state_value(ESTOP_DET);
}

/**
 * @brief Check the current state of the power button
 *
 * @return true if power button is pressed
 * @return false if power button is released
 */
static inline bool CheckPowerButtonState(void)
{
    return get_state_value(POW_SW_DET);
}

void relay_ctrl_process(void)
{
    #define ENTER_LOW_POWER_MODE_DELAY  10000

    static u8 state_machine = CHECK_DISASSEMBLE_STATE;
    static bool isDisassemble = false;
    static uint32_t count = 0;
    

    if(false == isGPIOStateUpdated)
    {
        return;
    }

    switch(state_machine)
    {
        case CHECK_DISASSEMBLE_STATE:
        //检测拆机信号
        if(true == CheckDisassembleState())
        {
            if(false == isDisassembleFlagUpdated)
            {
                isDisassemble = true;
                debug_print("disassemble event happend!!!\r\n");
                //发现拆机事件
                record_broken_event(0xFA);
                abnormal_flag = read_broken_event();
                isDisassembleFlagUpdated = true;
            }
        }
        else
        {
            isDisassemble = false;
        }
        state_machine = CHECK_24V_STATE;
        break;

        case CHECK_24V_STATE:
        //检测24V电源信号
        //有24V信号的情况下
        if(true == Check24VState())
        {
            state_machine = CHECK_ESTOP_STATE;
        }
        //没有24V信号的情况下
        else
        {
            //进入低功耗休眠模式
            debug_print("ready to enter standby mode\r\n");

            relay_ctrl_poll_time_count = 0;

            state_machine = IDLE_DELAY_STATE;
        }
        break;

        case CHECK_ESTOP_STATE:
        //急停按下
        if(true == CheckEstopState())
        {
            count++;
            if(count >= 1000000)
            {
                count = 0;
                debug_print("estop state>>>>>>>>>>>>>\r\n");
            }
            estop_relay_off();
            state_machine = CHECK_DISASSEMBLE_STATE;
        }
        //急停未按下
        else
        {
            state_machine = CHECK_POWER_BUTTON_STATE;
        }
        break;

        case CHECK_POWER_BUTTON_STATE:
        //电源按键按下
        if(true == CheckPowerButtonState())
        {
            count++;
            if(count >= 1000000)
            {
                count = 0;
                debug_print("power on>>>>>>>>>>>>>\r\n");
            }
            
            all_relay_on();
        }
        //电源按键未按下
        else
        {
            count++;
            if(count >= 1000000)
            {
                count = 0;
                debug_print("power off<<<<<<<<<<<<<<\r\n");
            }
            
            all_relay_off();
        }
        state_machine = CHECK_DISASSEMBLE_STATE;
        break;

        case IDLE_DELAY_STATE:
        if((relay_ctrl_poll_time_count >= ENTER_LOW_POWER_MODE_DELAY) || (true == isDisassemble))
        {
            relay_ctrl_poll_time_count = 0;

            disable_all_periph();
            enter_low_power();

            //低功耗进入空闲模式，唤醒后会重启单片机
            state_machine = IDLE_STATE;
        }
        else
        {
            if((relay_ctrl_poll_time_count % 1000) == 0)
            {
                debug_print("count down: this board will enter low power in %d second\r\n",(ENTER_LOW_POWER_MODE_DELAY-relay_ctrl_poll_time_count)/1000);
            }
        }
        break;

        case IDLE_STATE:
        break;

        default:
        break;
    }
}


uint32_t test_readwrite_result = 0;
void test_readwrite_backup_register(void)
{
    // record_broken_event(0xAA);
    // delay_1ms(1000);
    // test_readwrite_result = read_broken_event();
    // delay_1ms(1000);
    // record_broken_event(0);

    // uint32_t tmp = 0xAAAAAAAA;

    // flash_erase(break_up_register);
    // flash_write(break_up_register,&tmp,1);
    // delay_1ms(1000);
    flash_read(break_up_register,&test_readwrite_result,1);
}




